package org.sunspotworld;

import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.io.IScalarInput;
import java.io.IOException;
import java.util.Vector;

/**
 *
 * @author Administrator
 */
public class Maxbotics extends Thread {
    public static final int NUMSENSORS        = 4;
    EDemoBoard demo = EDemoBoard.getInstance();
    IScalarInput analogReadings[];
    private Vector sensors[] = new Vector[4];
    float lastReading[] = {2.0f, 2.0f, 2.0f, 2.0f};
    double a[] = {0.000221533, 1.0, 0.000214074, 1.0};
    double b[] = {-0.000159924, 1.0, -0.000252226, 1.0};
    
    public Maxbotics(  ){
        //TODO initialization needs to happen better
        sensors[0] = new Vector();
        sensors[1] = new Vector();
        sensors[2] = new Vector();
        sensors[3] = new Vector();
    }
    
    public void run() {
        try{
            while(true){
                
                int averagedReading;
                
                // get 100 readings for all the analog IR Sensors
                
                int[][] readings = new int[4][100];
                for( int reading = 0; reading < 100; reading++ ){
                    analogReadings = demo.getScalarInputs();
                    for( int sensor = 0; sensor < NUMSENSORS; sensor++){
                        readings[sensor][reading] = analogReadings[sensor].getValue();
                    }  
                }

                averagedReading = 0;
                for( int sensor = 0; sensor < NUMSENSORS; sensor++){
                    com.sun.squawk.util.Arrays.sort(readings[sensor]);
                    for( int i = 30; i < 100; i++ ){
                        averagedReading += readings[sensor][i];
                    }
                    averagedReading = averagedReading / 70;
                    newReading(sensor, (float)( averagedReading ) );
                    //System.out.println(sensor + " " + averagedReading); 
                }
                
       
                Thread.sleep(100);
            }
        }catch(Exception e){}
    }
    
    
    public float getReading(int z){
          //float reads[] = new float[3];
          //reads[0] = ((Float)sensors[z].elementAt(0)).floatValue();

          float voltage =  ((Float)sensors[z].elementAt(1)).floatValue();
          return voltageToDistance( voltage, a[z], b[z] );
    }
    
    // add the new reading
    private void newReading( int i, float reading ){
        // initialize the vectors
        // TODO: put this in a constructor
        float alpha = 0.3f;
        if( sensors[i].size() < 4 ){
            for( int z = 0; z < 4; z++ ){
                sensors[i].addElement(new Float(reading));
            }
        }
        
        //reading = (alpha * lastReading[i] ) + ( ( 1.0f - alpha ) * reading );
        //System.out.println("sensor weighted: " + i + " reading " + returnValue);
        lastReading[i] = reading;
        sensors[i].addElement(new Float(reading));
        sensors[i].removeElementAt(0);     
    }
    
    // raw adc reading of voltage really is an int
    // TODO change these to ints
    private float voltageToDistance( float voltage, double aIn, double bIn ){
        //return (float)( 1/(aIn * (double)voltage + bIn) - 0.42 );
        
        voltage = (voltage/ 1023.0f) * 3.0f;

        //return voltage;
        
        if(voltage >= 2.0f)
            return -2.0f * voltage + 9.0f;
        else if(voltage < 2.0f && voltage >= 1.4f)
            return -6.6666667f * voltage + 18.333f;
        else if(voltage < 1.4f && voltage >= 1.0f)
            return -10.0f * voltage + 23.0f;
        else if(voltage < 1.0f && voltage >= 0.8f)
            return -15.0f * voltage + 28.0f;
        else if(voltage < 0.8f && voltage >= 0.6f)
            return -30.0f * voltage + 40.0f;
        else if(voltage < 0.6f && voltage >= 0.42f)
            return -44.444f * voltage + 48.667f;
        else
            return -83.333f * voltage + 65f;
         
    }

}
